//
// Created by gg on 2023/10/6.
//
#ifdef __cplusplus
extern "C"{
#endif
#include "main.h"
#include "UPRE_MOTOR.h"
#include "stdio.h"
#include "string.h"
#include "UPRE_Drv_DjiMotor.h"
#include "cmsis_os2.h"
#include "UPRE_Decision.h"
extern  bool  flag_infrared_sensor;
#include "vesc.h"
#include "UPRE_Bsp_CAN.h"
#ifdef __cplusplus
};
#endif


DjiMotor M3508_1;
DjiMotor M3508_2;
DjiMotor M3508_3;
DjiMotor M3508_4;
DjiMotor M3508_5;
DjiMotor M3508_6;
DjiMotor M3508_7;

uint8_t RxBuff1[8];
uint8_t RxBuff2[8];
extern uint8_t recbuff[100];
CAN_RxHeaderTypeDef RxHeader1;
CAN_RxHeaderTypeDef RxHeader2;

void Motor_Init()
{

    M3508_1.can.CAN_Init(&hcan1);//电机CNA通信初始化
    M3508_1.Init( Control_Method_OMEGA, 19.0f, 10000.0f);//电机参数初始化
    M3508_1.PID_Omega.PID_ModInit(PID_Delta, 0, 500.0f, 10000.f, 0.0f);//PID功能初始化
    M3508_1.PID_Omega.PID_SET(40.0f,5.0f,15.0f,0.0f);//PID参数设定
//    M3508_1.PID_Angle.PID_ModInit(PID_Position, 0, 500.0f, 8000.f, 0.0f);
//    M3508_1.PID_Angle.PID_SET(150.0f,0.0f,5000.0f,1.0f);
    M3508_1.OUT = 0;
    ////////////////////////////////////////////////////////////////
    M3508_2.can.CAN_Init(&hcan1);//电机CNA通信初始化
    M3508_2.Init( Control_Method_OMEGA, 19.0f, 10000.0f);//电机参数初始化
    M3508_2.PID_Omega.PID_ModInit(PID_Delta, 0, 500.0f, 10000.f, 0.0f);//PID功能初始化
    M3508_2.PID_Omega.PID_SET(40.0f,5.0f,15.0f,0.0f);//PID参数设定
//    M3508_2.PID_Angle.PID_ModInit(PID_Position, 0, 500.0f, 8000.f, 0.0f);
//    M3508_2.PID_Angle.PID_SET(150.0f,0.0f,5000.0f,1.0f);
    M3508_2.OUT = 0;
    ////////////////////////////////////////////////////////////////
    M3508_3.can.CAN_Init(&hcan1);//电机CNA通信初始化
    M3508_3.Init( Control_Method_OMEGA, 19.0f, 10000.0f);//电机参数初始化
    M3508_3.PID_Omega.PID_ModInit(PID_Delta, 0, 500.0f, 10000.f, 0.0f);//PID功能初始化
    M3508_3.PID_Omega.PID_SET(40.0f,5.0f,15.0f,0.0f);//PID参数设定
//    M3508_1.PID_Angle.PID_ModInit(PID_Position, 0, 500.0f, 8000.f, 0.0f);
//    M3508_1.PID_Angle.PID_SET(150.0f,0.0f,5000.0f,1.0f);
    M3508_3.OUT = 0;
    ///////////////////////////////////////////////////////////////
    M3508_4.can.CAN_Init(&hcan1);//电机CNA通信初始化
    M3508_4.Init( Control_Method_OMEGA, 19.0f, 10000.0f);//电机参数初始化
    M3508_4.PID_Omega.PID_ModInit(PID_Delta, 0, 500.0f, 10000.f, 0.0f);//PID功能初始化
    M3508_4.PID_Omega.PID_SET(40.0f,5.0f,15.0f,0.0f);//PID参数设定
//    M3508_2.PID_Angle.PID_ModInit(PID_Position, 0, 500.0f, 8000.f, 0.0f);
//    M3508_2.PID_Angle.PID_SET(150.0f,0.0f,5000.0f,1.0f);
    M3508_4.OUT = 0;
    ///////////////////////////////////////////////////////////////
    M3508_5.can.CAN_Init(&hcan2);//电机CNA通信初始化
    M3508_5.Init( Control_Method_ANGLE, 19.0f, 10000.0f);//电机参数初始化
/*    M3508_5.PID_Omega.PID_ModInit(PID_Delta, 0, 500.0f, 10000.f, 0.0f);//PID功能初始化
    M3508_5.PID_Omega.PID_SET(10.0f,5.0f,10.0f,0.0f);//PID参数设定*/
    M3508_5.PID_Angle.PID_ModInit(PID_Position, 0, 4000.0f, 10000.f, 0.0f);
    M3508_5.PID_Angle.PID_SET(0.0f,0.0f,0.0f,1.0f);
    M3508_5.OUT = 0;
    ///////////////////////////////////////////////////////////////
    M3508_6.can.CAN_Init(&hcan2);//电机CNA通信初始化
    M3508_6.Init( Control_Method_ANGLE, 19.0f, 10000.0f);//电机参数初始化
/*    M3508_6.PID_Omega.PID_ModInit(PID_Delta, 0, 500.0f, 10000.f, 0.0f);//PID功能初始化
    M3508_6.PID_Omega.PID_SET(10.0f,5.0f,10.0f,0.0f);//PID参数设定*/
    M3508_6.PID_Angle.PID_ModInit(PID_Position, 0, 4000.0f, 10000.f, 0.0f);
    M3508_6.PID_Angle.PID_SET(0.0f,0.0f,0.0f,1.0f);
    M3508_6.OUT = 0;
    ///////////////////////////////////////////////////////////////


}
void Motor_DisInit()
{

    M3508_1.can.CAN_Init(&hcan1);//电机CNA通信初始化
    M3508_1.Init( Control_Method_OMEGA, 19.0f, 10000.0f);//电机参数初始化
    M3508_1.PID_Omega.PID_ModInit(PID_Delta, 0, 500.0f, 10000.f, 0.0f);//PID功能初始化
    M3508_1.PID_Omega.PID_SET(0.0f,0.0f,0.0f,0.0f);//PID参数设定
//    M3508_1.PID_Angle.PID_ModInit(PID_Position, 0, 500.0f, 8000.f, 0.0f);
//    M3508_1.PID_Angle.PID_SET(150.0f,0.0f,5000.0f,1.0f);
    M3508_1.OUT = 0;
    ////////////////////////////////////////////////////////////////
    M3508_2.can.CAN_Init(&hcan1);//电机CNA通信初始化
    M3508_2.Init( Control_Method_OMEGA, 19.0f, 10000.0f);//电机参数初始化
    M3508_2.PID_Omega.PID_ModInit(PID_Delta, 0, 500.0f, 10000.f, 0.0f);//PID功能初始化
    M3508_2.PID_Omega.PID_SET(0.0f,0.0f,0.0f,0.0f);//PID参数设定
//    M3508_2.PID_Angle.PID_ModInit(PID_Position, 0, 500.0f, 8000.f, 0.0f);
//    M3508_2.PID_Angle.PID_SET(150.0f,0.0f,5000.0f,1.0f);
    M3508_2.OUT = 0;
    ////////////////////////////////////////////////////////////////
    M3508_3.can.CAN_Init(&hcan1);//电机CNA通信初始化
    M3508_3.Init( Control_Method_OMEGA, 19.0f, 10000.0f);//电机参数初始化
    M3508_3.PID_Omega.PID_ModInit(PID_Delta, 0, 500.0f, 10000.f, 0.0f);//PID功能初始化
    M3508_3.PID_Omega.PID_SET(0.0f,0.0f,0.0f,0.0f);//PID参数设定
//    M3508_1.PID_Angle.PID_ModInit(PID_Position, 0, 500.0f, 8000.f, 0.0f);
//    M3508_1.PID_Angle.PID_SET(150.0f,0.0f,5000.0f,1.0f);
    M3508_3.OUT = 0;
    ///////////////////////////////////////////////////////////////
    M3508_4.can.CAN_Init(&hcan1);//电机CNA通信初始化
    M3508_4.Init( Control_Method_OMEGA, 19.0f, 10000.0f);//电机参数初始化
    M3508_4.PID_Omega.PID_ModInit(PID_Delta, 0, 500.0f, 10000.f, 0.0f);//PID功能初始化
    M3508_4.PID_Omega.PID_SET(0.0f,0.0f,0.0f,0.0f);//PID参数设定
//    M3508_2.PID_Angle.PID_ModInit(PID_Position, 0, 500.0f, 8000.f, 0.0f);
//    M3508_2.PID_Angle.PID_SET(150.0f,0.0f,5000.0f,1.0f);
    M3508_4.OUT = 0;
    ///////////////////////////////////////////////////////////////
    M3508_5.can.CAN_Init(&hcan1);//电机CNA通信初始化
    M3508_5.Init( Control_Method_ANGLE, 19.0f, 10000.0f);//电机参数初始化
/*    M3508_5.PID_Omega.PID_ModInit(PID_Delta, 0, 500.0f, 10000.f, 0.0f);//PID功能初始化
    M3508_5.PID_Omega.PID_SET(10.0f,5.0f,10.0f,0.0f);//PID参数设定*/
    M3508_5.PID_Angle.PID_ModInit(PID_Position, 0, 500.0f, 8000.f, 0.0f);
    M3508_5.PID_Angle.PID_SET(0,0.0f,0,1.0f);
    M3508_5.OUT = 0;
    ///////////////////////////////////////////////////////////////
    M3508_6.can.CAN_Init(&hcan1);//电机CNA通信初始化
    M3508_6.Init( Control_Method_ANGLE, 19.0f, 10000.0f);//电机参数初始化
/*    M3508_6.PID_Omega.PID_ModInit(PID_Delta, 0, 500.0f, 10000.f, 0.0f);//PID功能初始化
    M3508_6.PID_Omega.PID_SET(10.0f,5.0f,10.0f,0.0f);//PID参数设定*/
    M3508_6.PID_Angle.PID_ModInit(PID_Position, 0, 500.0f, 8000.f, 0.0f);
    M3508_6.PID_Angle.PID_SET(0,0.0f,0,1.0f);
    M3508_6.OUT = 0;
}

void Motor_ClearPID()
{
    M3508_1.PID_Omega.PID_SET(0.0f,0.0f,0.0f,0.0f);//PID参数设定
    ////////////////////////////////////////////////////////////////
    M3508_2.PID_Omega.PID_SET(0.0f,0.0f,0.0f,0.0f);//PID参数设定
    ////////////////////////////////////////////////////////////////
    M3508_3.PID_Omega.PID_SET(0.0f,0.0f,0.0f,0.0f);//PID参数设定
    ///////////////////////////////////////////////////////////////
    M3508_4.PID_Omega.PID_SET(0.0f,0.0f,0.0f,0.0f);//PID参数设定
    ///////////////////////////////////////////////////////////////
    M3508_5.PID_Angle.PID_SET(0,0.0f,0,1.0f);
    ///////////////////////////////////////////////////////////////
    M3508_6.PID_Angle.PID_SET(0,0.0f,0,1.0f);
}

void up_pid()
{

    M3508_5.PID_Angle.PID_SET(350,0.0f,12000,1.0f);
    ///////////////////////////////////////////////////////////////
    M3508_6.PID_Angle.PID_SET(350,0.0f,12000,1.0f);
}

void down_pid()
{
    M3508_5.PID_Angle.PID_SET(50,0.0f,7000,1.0f);
    ///////////////////////////////////////////////////////////////
    M3508_6.PID_Angle.PID_SET(50,0.0f,7000,1.0f);
}

void DjiMotor_Pos_arm_l(int32_t motor5)              ///单个位置
{
    M3508_5.Set_TargetParam(motor5, 0);
}

void DjiMotor_Pos_arm_r(int32_t motor6)              ///单个位置
{
    M3508_6.Set_TargetParam(motor6, 0);
}

void Motor_control(int32_t posi_arml,int32_t posi_armr,int32_t vel_daj,int32_t vel_vesc_trans,int32_t vesc_shoot)
{
#if Printf_Motor_Interface == 1
    printf("%d,%d,%d,%d,%f,%f\r\n",vesc_shoot,vel_vesc_trans,Pack_receive.is_accelerate,Pack_receive.is_decelerate,Pack_receive.Vel_x,Pack_receive.Vel_y);// 串口打印数据
#endif
    vesc_shoot     = vesc_shoot > 60000      ?  60000  : vesc_shoot;
    vesc_shoot     = vesc_shoot < 0          ?  0      : vesc_shoot;

    vel_vesc_trans = vel_vesc_trans > 4000   ?  4000   : vel_vesc_trans;
    Vesc_Set_ERPM(&hcan2,77, -vel_vesc_trans);
    Vesc_Set_Duty(&hcan2,100,-vesc_shoot);   //下
    HAL_Delay(1);
    DjiMotor_Pos_arm_l(posi_arml);
    HAL_Delay(1);
    DjiMotor_Pos_arm_r(posi_armr);

}

void DjiMotor_Speed(int32_t motor1,int32_t motor2,int32_t motor3,int32_t motor4)
{
    M3508_4.Set_TargetParam(0, -motor4);
    M3508_1.Set_TargetParam(0, -motor1);
    M3508_2.Set_TargetParam(0, -motor2);
    M3508_3.Set_TargetParam(0, -motor3);

    M3508_3.Set_TargetParam(0, -motor3);
    M3508_4.Set_TargetParam(0, -motor4);
    M3508_3.Set_TargetParam(0, -motor3);
    M3508_1.Set_TargetParam(0, -motor1);
    M3508_2.Set_TargetParam(0, -motor2);
}
/*
 * @brief 串口发送数据包
 * @param[in]... 电机需要显示数据
 */
//void Pack(uint8_t *buf)
//{
//    sscanf((const char*)buf,"%f %f",&M3508_7.Target_Omega,&M3508_2.Target_Omega);
//}
#ifdef __cplusplus
extern "C"{

/*
 * @brief CANFifo0的接收回调函数,用于更改DJI电驱接收ID
 * @param[in]... 更改DJI电调ID 0x20+电机编号
 */
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    if(hcan->Instance == CAN1)
    {
        if(HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader1, RxBuff1)==HAL_OK) {
            switch (RxHeader1.StdId) {

                case 0x201:{
                    M3508_1.Get_Moter_Info(&hcan1, RxBuff1);//在CAN回调中不断获取电机反馈数据
                    M3508_1.Control_PID();
                }break;

                case 0x202:{
                    M3508_2.Get_Moter_Info(&hcan1, RxBuff1);//在CAN回调中不断获取电机反馈数据
                    M3508_2.Control_PID();
                }break;

                case 0x203:{
                    M3508_3.Get_Moter_Info(&hcan1, RxBuff1);//在CAN回调中不断获取电机反馈数据
                    M3508_3.Control_PID();
                }break;

                case 0x204:{
                    M3508_4.Get_Moter_Info(&hcan1, RxBuff1);//在CAN回调中不断获取电机反馈数据
                    M3508_4.Control_PID();
                }break;

            }
            __HAL_CAN_ENABLE_IT(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
        }
    }
    if(hcan->Instance == CAN2)
    {
        if(HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader2, RxBuff2)==HAL_OK) {
            switch (RxHeader2.StdId) {
                case 0x205: {
                    M3508_5.Get_Moter_Info(&hcan2, RxBuff2);//在CAN回调中不断获取电机反馈数据
                    M3508_5.Control_PID();
                }
                    break;

                case 0x206: {
                    M3508_6.Get_Moter_Info(&hcan2, RxBuff2);//在CAN回调中不断获取电机反馈数据
                    M3508_6.Control_PID();
                }
                    break;
            }

            __HAL_CAN_ENABLE_IT(&hcan2, CAN_IT_RX_FIFO0_MSG_PENDING);

        }
    }


}
/*
 * @brief CANFifo0的接收回调函数,用于更改DJI电驱接收ID
 * @param[in]... 更改DJI电调ID 0x20+电机编号
 */
void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan)
{

}

#endif
#ifdef __cplusplus
};
#endif

/*
 * @brief Function for CAN_Filter configuration
 * @param hcan: pointer
 * @retval None
 */
void CAN_FilterInit(CAN_HandleTypeDef *hcan)
{
    CAN_FilterTypeDef		CAN_FilterConfigStructure;

    /*filter config for can2*/
    CAN_FilterConfigStructure.FilterBank = 0;                      // filter 0
    CAN_FilterConfigStructure.FilterMode = CAN_FILTERMODE_IDMASK;  // mask mode
    CAN_FilterConfigStructure.FilterScale = CAN_FILTERSCALE_32BIT;
    CAN_FilterConfigStructure.FilterIdHigh = 0x0000;
    CAN_FilterConfigStructure.FilterIdLow = 0x0000;
    CAN_FilterConfigStructure.FilterMaskIdHigh = 0x0000;
    CAN_FilterConfigStructure.FilterMaskIdLow = 0x0000;          // set mask 0 to receive all can id
    CAN_FilterConfigStructure.FilterFIFOAssignment = CAN_RX_FIFO0; // assign to fifo0
    CAN_FilterConfigStructure.FilterActivation = ENABLE;           // enable can filter
    CAN_FilterConfigStructure.SlaveStartFilterBank = 14;           // only meaningful in dual can mode

    if (HAL_CAN_ConfigFilter(hcan, &CAN_FilterConfigStructure) != HAL_OK)
    {
        Error_Handler();
    }
    /* Start the CAN peripheral */
    if (HAL_CAN_Start(hcan) != HAL_OK)
    {
        /* Start Error */
        Error_Handler();
    }

    /* Activate CAN RX notification */
    if (HAL_CAN_ActivateNotification(hcan, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK)
    {
        /* Notification Error */
        Error_Handler();
    }

}

